#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  HTMotor)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorR,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motorL,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorCL,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     motorCR,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C4_1,     motorBall,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     motorI,        tmotorNone, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    Bridge,               tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    Lift,                 tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    Lift2,                tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/**
 * Project Name: Teleop8
 * Version: 1
 *
 * This teleop file was generated using Teleop Catalyst for RobotC Version 1.3.1 beta on 2/1/2012.
 * Learn more about Teleop Catalyst at: http://ftc.kcastromechs.org/catalyst
 *
 * Feel free to modify this code as much as you'd like, as long as you keep this comment
 * block, in its unaltered form, somewhere in your code. Thanks!
**/

//// Include Files:
#include "JoystickDriver.c";

//// Function Prototypes:
// Initialize function (auto-executed when teleop period begins):
void initialize();

// "Event Handlers" for gamepad 1:
void g1_btn5_t_handler();
void g1_btn6_t_handler();

// "Event Handlers" for gamepad 2:
void g2_btn5_c_handler();
void g2_btn7_c_handler();
void g2_btn6_c_handler();
void g2_btn8_c_handler();
void g2_btn1_t_handler();
void g2_btn2_t_handler();

// Execute function(s) for gamepad 1:
void g1_btn5_t_execute();
void g1_btn6_t_execute();

// Execute function(s) for gamepad 2:
void g2_btn5_c_execute();
void g2_btn7_c_execute();
void g2_btn6_c_execute();
void g2_btn8_c_execute();
void g2_btn1_t_execute();
void g2_btn2_t_execute();

float deadband(float power);


//// Global Booleans:
// Booleans for Gamepad 1:
bool g1_btn5_toggledOn = false;
bool g1_btn5_flag = false;
bool g1_btn6_toggledOn = false;
bool g1_btn6_flag = false;
// Booleans for Gamepad 2:
bool g2_btn5_flag = false;
bool g2_btn7_flag = false;
bool g2_btn6_flag = false;
bool g2_btn8_flag = false;
bool g2_btn1_toggledOn = false;
bool g2_btn1_flag = false;
bool g2_btn2_toggledOn = false;
bool g2_btn2_flag = false;

/// Joystick values:
float g1_joy1_y1;
float g1_joy1_y2;
float g2_joy1_y1;
float g2_joy1_y2;

/// Variables for driveSwap, slowDown, and deadban features:
float deadbandValue  = 10;   // Deadband is a "neutral zone" where nothing happens.
float slowDownFactor = 1;    // May be used to reduce the speed of the robot/increase sensitivity.
float joySwap = 1;           // May be used to swap the driving direction of the robot. 1 means forward = forward, -1 means forward = backwards

task main()
{
   initialize();   // Run the initialization code.

   while (true)
   {
      getJoystickSettings(joystick);

      if (joystick.UserMode == true)
      {
         g1_btn5_t_handler();
         g1_btn6_t_handler();
         g2_btn5_c_handler();
         g2_btn7_c_handler();
         g2_btn6_c_handler();
         g2_btn8_c_handler();
         g2_btn1_t_handler();
         g2_btn2_t_handler();

         // Update variables with joystick values:
         g1_joy1_y1 = joystick.joy1_y1;
         g1_joy1_y2 = joystick.joy1_y2;
         g2_joy1_y1 = joystick.joy2_y1;
         g2_joy1_y2 = joystick.joy2_y2;

         // Update the power of the drive motors with that of the joystick values:
         motor[motorL] = (deadband(g1_joy1_y1/slowDownFactor)*joySwap);
         motor[motorR] = (deadband(g1_joy1_y2/slowDownFactor)*joySwap);
      }
   }
}

void initialize()
{

}

float deadband(float power) {
   if (abs(power) < deadbandValue) {
      return 0;
   } else {
      return power;
   }
}

void g1_btn5_t_handler()
{
   if (joy1Btn(5)) {
      g1_btn5_flag = true;
   } else if (g1_btn5_flag) {
      g1_btn5_flag = false;
      if (g1_btn5_toggledOn) {
         g1_btn5_toggledOn = false;
      } else {
         g1_btn5_toggledOn = true;
      }
      g1_btn5_t_execute();
   }
}

void g1_btn6_t_handler()
{
   if (joy1Btn(6)) {
      g1_btn6_flag = true;
   } else if (g1_btn6_flag) {
      g1_btn6_flag = false;
      if (g1_btn6_toggledOn) {
         g1_btn6_toggledOn = false;
      } else {
         g1_btn6_toggledOn = true;
      }
      g1_btn6_t_execute();
   }
}

void g2_btn5_c_handler()
{
   if (joy2Btn(5)) {
      g2_btn5_c_execute();
      g2_btn5_flag = true;
   } else if (g2_btn5_flag) {
      motor[motorCL] = 0;
      g2_btn5_flag = false;
   }
}

void g2_btn7_c_handler()
{
   if (joy2Btn(7)) {
      g2_btn7_c_execute();
      g2_btn7_flag = true;
   } else if (g2_btn7_flag) {
      motor[motorCL] = 0;
      g2_btn7_flag = false;
   }
}

void g2_btn6_c_handler()
{
   if (joy2Btn(6)) {
      g2_btn6_c_execute();
      g2_btn6_flag = true;
   } else if (g2_btn6_flag) {
      motor[motorCR] = 0;
      g2_btn6_flag = false;
   }
}

void g2_btn8_c_handler()
{
   if (joy2Btn(8)) {
      g2_btn8_c_execute();
      g2_btn8_flag = true;
   } else if (g2_btn8_flag) {
      motor[motorCR] = 0;
      g2_btn8_flag = false;
   }
}

void g2_btn1_t_handler()
{
   if (joy2Btn(1)) {
      g2_btn1_flag = true;
   } else if (g2_btn1_flag) {
      g2_btn1_flag = false;
      if (g2_btn1_toggledOn) {
         g2_btn1_toggledOn = false;
      } else {
         g2_btn1_toggledOn = true;
      }
      g2_btn1_t_execute();
   }
}

void g2_btn2_t_handler()
{
   if (joy2Btn(2)) {
      g2_btn2_flag = true;
   } else if (g2_btn2_flag) {
      g2_btn2_flag = false;
      if (g2_btn2_toggledOn) {
         g2_btn2_toggledOn = false;
      } else {
         g2_btn2_toggledOn = true;
      }
      g2_btn2_t_execute();
   }
}

void g1_btn5_t_execute()
{
   if (g1_btn5_toggledOn)
   {
      servo[Lift2] = 8;
   }
   else
   {
      servo[Lift2] = 248;
   }
}

void g1_btn6_t_execute()
{
   if (g1_btn6_toggledOn)
   {
      servo[Lift] = 248;
   }
   else
   {
      servo[Lift] = 8;
   }
}

void g2_btn5_c_execute()
{
   motor[motorCL] = -90
}

void g2_btn7_c_execute()
{
   motor[motorCL] = 10
}

void g2_btn6_c_execute()
{
   motor[motorCR] = 85;
}

void g2_btn8_c_execute()
{
   motor[motorCR] = -10;
}

void g2_btn1_t_execute()
{
   if (g2_btn1_toggledOn)
   {
      motor[motorBall] = 90;
   }
   else
   {
      motor[motorBall] = 0;
   }
}

void g2_btn2_t_execute()
{
   if (g2_btn2_toggledOn)
   {
      servo[Bridge] = 248;
   }
   else
   {
      servo[Bridge] = 128;
   }
}

// That's it! Enjoy, and good luck! :)
